// --------------------------------------------------------------------------------------------------------------------
// <copyright file="VirtualDriveFeedback.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Runtime.Perception
{
    using System;
    using System.Diagnostics;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Navigation.Runtime;
    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Agent that maintains an estimation of robot pose (x, y, heading) given drive odometry along
    /// with angular speed on the y-axis of the gyro.
    /// </summary>
    public class VirtualDriveFeedback
    {
        /// <summary>
        /// The magnitude above which to ignore readings during gyro calibration
        /// </summary>
        private readonly double gyroCalibrationThreshold;

        /// <summary>
        /// Trust gyro drift above a threshold if odometry says we're not turning at all.
        /// This happens when then robot comes to a sudden stop but momentum carries it further.
        /// It also happens when wheel speeds are meant to be equal but the robot actually drifts
        /// ever-so-slightly to one side. Also, sometimes the wheels "settle" into the next stepper
        /// increment after stopping.
        /// </summary>
        /// <remarks>
        /// This is used only when odometry is reporting *zero* motion.
        /// </remarks>
        private readonly double driftThresholdRadiansPerSecond;

        /// <summary>
        /// If odometry says we're turning more than gyro reports, treat as wheel slip.
        /// Threshold for delta between what odometry and gyro are reporting above which we
        /// suspect the wheels are slipping and trust the gyro.
        /// </summary>
        /// <remarks>
        /// This is used only when odometry is reporting *some* motion. Otherwise we assume
        /// the gyro is drifting (or perhaps someone is pushing the robot).
        /// </remarks>
        private readonly double slipThresholdPercentage;

        /// <summary>
        /// Calibration factor determined experimentally.
        /// </summary>
        private readonly double gyroYCalibrationFactor;

        /// <summary>
        /// Internal IMU state.
        /// </summary>
        private readonly IMUState currentIMUState;

        /// <summary>
        /// Previous drive feedback state.
        /// </summary>
        private DriveFeedbackState lastDriveFeedbackState;

        /// <summary>
        /// Current robot heading.
        /// </summary>
        private double heading;

        /// <summary>
        /// Last gyro message timestamp.
        /// </summary>
        /// <remarks>
        /// It is necessary to know the elapsed time between gyro samples in order to
        /// integrate the gyro (angular speed) over time into radians/sec.
        /// </remarks>
        private long lastGyroOriginatingTimeInMilliseconds = long.MaxValue;

        /// <summary>
        /// Initializes a new instance of the VirtualDriveFeedback class.
        /// </summary>
        /// <param name="gyroCalibrationThreshold">Calibration threshold</param>
        /// <param name="driftThresholdRadiansPerSecond">Drift threshold</param>
        /// <param name="slipThresholdPercentage">Slip threshold</param>
        /// <param name="gyroYCalibrationFactor">Gyro-Y calibration factor</param>
        /// <param name="requiredGyroMeasurementsForCalibration">Number of required samples to determine bias.</param>
        public VirtualDriveFeedback(
            double gyroCalibrationThreshold,
            double driftThresholdRadiansPerSecond,
            double slipThresholdPercentage,
            double gyroYCalibrationFactor,
            int requiredGyroMeasurementsForCalibration)
        {
            this.gyroCalibrationThreshold = gyroCalibrationThreshold;
            this.driftThresholdRadiansPerSecond = driftThresholdRadiansPerSecond;
            this.slipThresholdPercentage = slipThresholdPercentage;
            this.gyroYCalibrationFactor = gyroYCalibrationFactor;
            this.currentIMUState = new IMUState(requiredGyroMeasurementsForCalibration, gyroCalibrationThreshold);
        }

        /// <summary>
        /// Calculate gyro-based heading delta.
        /// </summary>
        /// <param name="gyroMessage">Gyro message.</param>
        /// <returns>Heading delta in radians.</returns>
        public double GyroHeadingDelta(GyroscopeMessage gyroMessage)
        {
            double lastGyroYReading = this.currentIMUState.GyroYInRadiansPerSecond;
            this.currentIMUState.IMUHeartbeatUpdate(null, gyroMessage.State[0]);
            long lastOriginatingTime = this.lastGyroOriginatingTimeInMilliseconds;
            long originatingTime = gyroMessage.OriginatingTime;
            this.lastGyroOriginatingTimeInMilliseconds = originatingTime;

            if (this.currentIMUState.GyroBiasCalibrationReadingsRemaining == 0 && lastOriginatingTime < originatingTime)
            {
                // not still calibrating and not first or int overflow
                double start = lastGyroYReading * this.gyroYCalibrationFactor;
                double ellapsedSeconds = TimeSpan.FromMilliseconds(originatingTime - lastOriginatingTime).TotalSeconds;
                double end = this.currentIMUState.GyroYInRadiansPerSecond * this.gyroYCalibrationFactor;

                return ellapsedSeconds * (start + end) / 2;
            }

            return 0;
        }

        /// <summary>
        /// Calculate odometry based heading delta.
        /// </summary>
        /// <param name="driveFeedbackState">Drive feedback state.</param>
        /// <returns>Odometry delta pose.</returns>
        public Pose2D GetOdometryDelta(DriveFeedbackState driveFeedbackState)
        {
            if (this.lastDriveFeedbackState == null)
            {
                return new Pose2D();
            }

            return driveFeedbackState.GetMeanDeltaPose(this.lastDriveFeedbackState);
        }

        /// <summary>
        /// Get updated drive feedback state.
        /// </summary>        
        /// <param name="odometryDelta">Odometry delta</param>
        /// <param name="gyroDelta">Gyro delta.</param>
        /// <param name="driveFeedbackState">Drive feedback message.</param>
        /// <returns>Updated drive feedback state.</returns>
        public DriveFeedbackState GetUpdatedDriveFeedbackState(double odometryDelta, double gyroDelta, DriveFeedbackState driveFeedbackState)
        {
            if (this.lastDriveFeedbackState == null)
            {
                this.lastDriveFeedbackState = driveFeedbackState;
                return driveFeedbackState;
            }            

            DriveFeedbackState updatedDriveStateFeedback;
            if (Math.Abs(odometryDelta) > 0)
            {
                ////if (this.currentIMUState.GyroBiasCalibrationReadingsRemaining > 0)
                ////{
                ////    throw new InvalidOperationException("Driving before gyro calibration complete!");
                ////}

                // turning? If odometry says we're turning more than gyro reports, treat as wheel slip.
                bool slipping = Math.Abs((odometryDelta / gyroDelta) - 1) > this.slipThresholdPercentage;

                updatedDriveStateFeedback = this.UpdatePose(slipping ? gyroDelta : odometryDelta, driveFeedbackState);

                ////Console.Write(slipping ? '!' : '.');
            }
            else
            {
                // Not turning. Trust gyro drift above a threshold if odometry says we're not turning at all.
                double y = this.currentIMUState.GyroYInRadiansPerSecond;
                bool drifting = Math.Abs(y) > this.driftThresholdRadiansPerSecond;
                updatedDriveStateFeedback = this.UpdatePose(drifting ? gyroDelta : odometryDelta, driveFeedbackState);

                ////Console.Write(drifting ? '?' : '.');
            }

            this.lastDriveFeedbackState = updatedDriveStateFeedback;
            return updatedDriveStateFeedback;
        }

        /// <summary>
        /// Update pose, given trusted heading delta and drive odometry.
        /// </summary>
        /// <param name="headingDelta">Heading delta (from gyro or odometry).</param>
        /// <param name="drive">Drive feedback state.</param>
        /// <returns>Update Drive feedback state incorporating heading information</returns>
        private DriveFeedbackState UpdatePose(double headingDelta, DriveFeedbackState drive)
        {
            this.heading = this.heading + headingDelta;
            return drive.CreateDriveFeedbackState(this.heading);
        }
    }
}
